#include "xfcanetdriver/canetReceive.h"
xfcanetReceive::xfcanetReceive(ros::NodeHandle &m_handle)
{ // 构造函数初始化UDP端口

  ros::NodeHandle private_nh("~");

  private_nh.param<std::string>("m_pcip", m_pcip, "192.168.1.4");
  private_nh.param<int>("m_pcport", m_pcport, 4001);

  private_nh.param<int>("bdebug", bdebug, 0);

  if (bdebug > 0)
  {
    // paramters
    std::cout << FYEL("*****xfcanet-2e-u:parameters*******************")
              << std::endl;
    std::cout << FGRN("pc_ip: ") << m_pcip << std::endl;
    std::cout << FGRN("pc_port: ") << m_pcport << std::endl;
    std::cout << FYEL("*****xfcanet-2e-u:parameters end***************")
              << std::endl;
  }
  pub_control = m_handle.advertise<xfcanetdriver::canInfo>("/xfcandriver",
                                                           1); // 发布节点
}

xfcanetReceive::~xfcanetReceive() // 析构函数，在停止对象调用时调用
{
  delete boost_udp; // 释放UDP调用内存
}

void xfcanetReceive::run()
{ // 主函数

  // bind socket and receive_data
  boost::asio::io_service io_service;
  boost_udp = new Boost_UDP_Receive(io_service, m_pcip, m_pcport);

  ROS_INFO("xfcanetReceive running! ");

  ros::Rate rate(1000);

  while (ros::ok())
  {
    RecvCarInfoKernel(); // canet -->pc
    rate.sleep();
  }
}
void xfcanetReceive::RecvCarInfoKernel()
{ // 接受CANet的数据

  memset(buffer, 0, sizeof(buffer));                         // 缓存buffer清零
  int ret = boost_udp->receive_data(buffer, RECVBUFFERSIZE); // ret表示的是数据长度的字节数
  // cout<<"ret : " << ret <<endl;


  if (ret > 0) // 13　　接收13的倍数个字节的数据
  {
    ParserData(buffer, ret); // 一次收到的有多帧数据,我们需要解析。
                             // 边收边解析，每接受到13个字节的数据会进行一次解析
  }
}

void xfcanetReceive::ParserData(
    unsigned char data[],
    int num) // buffer是数据缓存数组，num是数据接收的字节数．
{

  bool received = false;
  for (int i = 0; i < num / 13; ++i) // 从第1３个字节以上
  {

    stCANMsg frame; // 接收到的数据报文信息，包头＋ID + 数据
    CHAR2UINT ii;

    for (int j = 0; j < 4; ++j)
    {
      ii.ch[3 - j] = data[1 + j + i * 13];
    }
    unsigned int id = ii.i; //
    frame.ID = id;
    for (int j = 0; j < 8; ++j)
    {
      frame.data[j] = data[5 + j + i * 13];
    }

    if (frame.ID == 0x502)
    {
      received = true;
      car_info.FeedBack_CAR_Method = frame.data[0]; // 0:驻车 P 1:手动驾驶（摇杆）PM 2:手动驾驶（语音）M 3:遥控驾驶 R     4:遥控驻车 PR 5:自动驾驶 A
      car_info.FeedBack_EPS_Angle = frame.data[1];  // 模拟摇杆X轴百分比 左负右正
      car_info.MCU_VehicleSpeed = frame.data[2];    // 模拟摇杆Y轴百分比 下负上正
      // pose_nav.twist.twist.linear.x = car_info.MCU_VehicleSpeed;
    }
    else if (frame.ID == 0x503)
    {
      received = true;
      car_info.Module_RMC_Sts = (frame.data[0] >> 0) & 0x01;
      car_info.Module_VOL_Sts = (frame.data[0] >> 1) & 0x01;
      car_info.Module_4G_Sts = (frame.data[0] >> 2) & 0x01;
      car_info.Module_GOY_Sts = (frame.data[0] >> 3) & 0x01;
      car_info.RealLamp1 = (frame.data[0] >> 4) & 0x01;
      car_info.RealLamp2 = (frame.data[0] >> 5) & 0x01;
      car_info.RealLamp3 = (frame.data[0] >> 6) & 0x01;
      car_info.BMS_BatterySoc = frame.data[1]; // 电池电压 0-30V
    }
    if(bdebug)
      ROS_INFO("xfcanetReceive: %d %x ", num, frame.ID);
    }


  if (received)
  {
    if (bdebug)
      ROS_INFO("FeedBack_CAR_Method:%d", car_info.FeedBack_CAR_Method);

    pub_control.publish(car_info);
  }
}
